
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <fcntl.h>
#include <ctype.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/mman.h>

#include "stdio.h"
#include "stdint.h"
#include <errno.h>
#include <string.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <memory.h>
#include <sys/mman.h>

#define PAGE_SIZE 4096
#define PAGE_MASK (PAGE_SIZE - 1)

uint8_t m_write[1024];
uint8_t m_read[1024];


#define QLOG_LINE_BUF_SIZE 256
#define QLOG_NEWLINE_SIGN "\n"
/**
 * another copy string function
 *
 * @param cur_len current copied log length, max size is ELOG_LINE_BUF_SIZE
 * @param dst destination
 * @param src source
 *
 * @return copied length
 */
static size_t qlog_strcpy(size_t cur_len, char *dst, const char *src) {
    const char *src_old = src;

    // assert(dst);
    // assert(src);

    while (*src != 0) {
        /* make sure destination has enough space */
        if (cur_len++ < QLOG_LINE_BUF_SIZE) {
            *dst++ = *src++;
        } else {
            break;
        }
    }
    return src - src_old;
}
/**
 * @brief: 输出指定长度的数组数据
 * @param {const char *} name 本次输出的名称
 * @param {uint8} width：指定输出的宽度，多少个字节一行
 * @param {uint8} buf：需要输出的数据
 * @param {uint16} size：需要输出的长度
 * @return {*}
 */

void qlog_hexdump(const char *name, uint8_t width, uint8_t *buf, uint16_t size)
{
#define __is_print(ch)       ((unsigned int)((ch) - ' ') < 127u - ' ')


    uint16_t i, j;
    uint16_t log_len = 0;
    char dump_string[8] = {0};
    int fmt_result;
    static char log_buf[QLOG_LINE_BUF_SIZE];

    for (i = 0; i < size; i += width) {
        /* package header */
        fmt_result = snprintf(log_buf, QLOG_LINE_BUF_SIZE, "D/HEX %s: %04X-%04X: ", name, i, i + width - 1);
        /* calculate log length */
        if ((fmt_result > -1) && (fmt_result <= QLOG_LINE_BUF_SIZE)) {
            log_len = fmt_result;
        } else {
            log_len = QLOG_LINE_BUF_SIZE;
        }
        /* dump hex */
        for (j = 0; j < width; j++) {
            if (i + j < size) {
                snprintf(dump_string, sizeof(dump_string), "%02X ", buf[i + j]);
            } else {
                strncpy(dump_string, "   ", sizeof(dump_string));
            }
            log_len += qlog_strcpy(log_len, log_buf + log_len, dump_string);
            if ((j + 1) % 8 == 0) {
                log_len += qlog_strcpy(log_len, log_buf + log_len, " ");
            }
        }
        log_len += qlog_strcpy(log_len, log_buf + log_len, "  ");
        /* dump char for hex */
        for (j = 0; j < width; j++) {
            if (i + j < size) {
                snprintf(dump_string, sizeof(dump_string), "%c", __is_print(buf[i + j]) ? buf[i + j] : '.');
                log_len += qlog_strcpy(log_len, log_buf + log_len, dump_string);
            }
        }
        /* overflow check and reserve some space for newline sign */
        if (log_len + strlen(QLOG_NEWLINE_SIGN) > QLOG_LINE_BUF_SIZE) {
            log_len = QLOG_LINE_BUF_SIZE - strlen(QLOG_NEWLINE_SIGN);
        }
        /* package newline sign */
        log_len += qlog_strcpy(log_len, log_buf + log_len, QLOG_NEWLINE_SIGN);
        /* do log output */
        printf("%s\n",log_buf);
    }

}

void main(int argc, char **argv)
{
    int fd;
	unsigned long target = 0xa0050000;
    if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1) 
    {
        printf("/dev/mem opened error.\n"); 
        return ;
    };
    printf("/dev/mem opened.\n"); 
    fflush(stdout);
	if(argc > 2) {
		target = strtoul(argv[1], 0, 0);//配置入口地址
	}
    memorySwap(fd,target, m_read, 1024, m_write, 0);
	qlog_hexdump("SXK status:",4,m_read,256);

}


/**
 * @description: 用于交换内存的函数，2023-6-6修改添加偏移量设置，修改翻页
 * @param {int} fd
 * @param {unsigned long} phyaddr
 * @param {unsigned char*} rbuf
 * @param {int} rlen
 * @param {unsigned char*} wbuf
 * @param {int} wlen
 * @return {*}
 */
int memorySwap(int fd,unsigned long phyaddr, unsigned char* rbuf, uint64_t rlen, unsigned char* wbuf, uint64_t wlen)
{
    uint64_t offset_page = 0,offset_len = 0,len = 0,addr_offset = 0,i = 0;
	uint64_t g_aByte = 0,g_bByte=0;
	uint64_t g_Length=0;
    void *map_base, *virt_addr;
	off_t addr = phyaddr;
	if(fd < 0){
		printf("memory_swap fd is error\n");
 		return -1;
	}
	//Calculated addr offset
	addr_offset = addr%PAGE_SIZE;
	//Get base addr to Get virtual address
	addr = addr - addr_offset;

	if(rlen%8 != 0){
		printf("phyaddr:0x%#lx read not 8n byte\n",phyaddr);
		return -1;
	}
	if(wlen%8 != 0){
		printf("phyaddr:%#lx write not 8n byte\n",phyaddr);
		return -1;
	}

	if(wbuf != NULL){
		if(wlen > 0){
			//先计算出需要几页
			len = wlen;
			g_aByte=0;g_bByte=0;
			g_aByte = PAGE_SIZE - addr_offset;//第一页剩余字节
			if (len > g_aByte) {
				g_bByte=len-g_aByte;
			} else {
				offset_page=1;
			}
			if(g_bByte > 0){
				offset_page = g_bByte/PAGE_SIZE + 1;//+1是第一页剩余长度
				if(g_bByte%PAGE_SIZE>0){
					offset_page = offset_page+1;//+1是最后一页剩余长度
				}
			}
			offset_len = 0;
			for(i=0;i<offset_page;i++)
			{
				/* 申请一页 */ //将内核空间映射到用户空间
				map_base = mmap(0, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, addr);
				if(map_base == (void *) -1)
				{
					// fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					printf("Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					close(fd);
					return 0;
				}

				//基地址加上数据偏移量
				if(i == 0){
					virt_addr = ((uint8_t *)map_base+addr_offset);
					if(len>=g_aByte) {
						g_Length = g_aByte;
					}else {
						g_Length=len;
					}
				}else{//剩余要写入的字节
					virt_addr = map_base;
					if(len >= PAGE_SIZE){
						g_Length=PAGE_SIZE;
					} else {
						g_Length=len;
					}
				}
				// printf("write addr_offset:%d,g_Length:%d\r\n",addr_offset,g_Length);
				memcpy(((unsigned char *) virt_addr),(wbuf+offset_len),g_Length);
				if(munmap(map_base, PAGE_SIZE) == -1)
				{
					// fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					printf("Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					close(fd);
					return 0;
				}
				offset_len += g_Length;
				addr += PAGE_SIZE;
				len -= g_Length;
			}

		}
	}

	if(rbuf != NULL){
		if(rlen > 0){
			//先计算出需要几页
			len = rlen;
			addr = phyaddr - addr_offset;
			g_aByte=0;g_bByte=0;
			g_bByte = 0;
			g_aByte = PAGE_SIZE - addr_offset;//第一页剩余字节
			if(len > g_aByte){
				g_bByte=len-g_aByte;
			} else {
				offset_page=1;
			}
			if(g_bByte > 0){
				offset_page = g_bByte/PAGE_SIZE + 1;//+1是第一页剩余长度
				if(g_bByte%PAGE_SIZE>0){
					offset_page = offset_page+1;//+1是最后一页剩余长度
				}
			}  
			offset_len = 0;
			for(i=0;i<offset_page;i++)
			{
				/* 申请一页 */ //将内核空间映射到用户空间
				map_base = mmap(0, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, addr);
				if(map_base == (void *) -1)
				{
					// fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					printf("Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					close(fd);
					return 0;
				}

				//基地址加上数据偏移量
				if(i == 0){
					virt_addr = ((uint8_t *)map_base+addr_offset);
					if(len>=g_aByte) {
						g_Length = g_aByte;
					} else {
						g_Length=len;
					}
				}else{//剩余要读取的字节
					virt_addr = map_base;
					if(len >= PAGE_SIZE) {
						g_Length=PAGE_SIZE;
					} else {
						g_Length=len;
					}
				}
				// printf("read addr_offset:%d,g_Length:%d\r\n",addr_offset,g_Length);
				memcpy((rbuf+offset_len),((unsigned char *) virt_addr),g_Length);
				if(munmap(map_base, PAGE_SIZE) == -1)
				{
					// fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					printf("Error at line %d, file %s (%d) [%s]\n", __LINE__, __FILE__, errno, strerror(errno));
					close(fd);
					return 0;
				}
				offset_len += g_Length;
				addr += PAGE_SIZE;
				len -= g_Length;
			}
		}
	}
	return 0;
}


  
#define FATAL do { fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", \
  __LINE__, __FILE__, errno, strerror(errno)); exit(1); } while(0)
 
#define MAP_SIZE 4096UL
#define MAP_MASK (MAP_SIZE - 1)

int main000(int argc, char **argv) {
    int fd;
    void *map_base, *virt_addr; 
	unsigned long read_result, writeval;
	off_t target;
	int access_type = 'w';
	
	if(argc < 2) {
		fprintf(stderr, "\nUsage:\t%s { address } [ type [ data ] ]\n"
			"\taddress : memory address to act upon\n"
			"\ttype    : access operation type : [b]yte, [h]alfword, [w]ord\n"
			"\tdata    : data to be written\n\n",
			argv[0]);
		exit(1);
	}
	target = strtoul(argv[1], 0, 0);

	if(argc > 2)
		access_type = tolower(argv[2][0]);


    if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1) FATAL;
    printf("/dev/mem opened.\n"); 
    fflush(stdout);
    
    /* Map one page */
    map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
    if(map_base == (void *) -1) FATAL;
    printf("Memory mapped at address %p.\n", map_base); 
    fflush(stdout);
    
    virt_addr = map_base + (target & MAP_MASK);
    switch(access_type) {
		case 'b':
			read_result = *((unsigned char *) virt_addr);
			break;
		case 'h':
			read_result = *((unsigned short *) virt_addr);
			break;
		case 'w':
			read_result = *((unsigned long *) virt_addr);
			break;
		default:
			fprintf(stderr, "Illegal data type '%c'.\n", access_type);
			exit(2);
	}
    printf("Value at address 0x%X (%p): 0x%X\n", target, virt_addr, read_result); 
    fflush(stdout);

	if(argc > 3) {
		writeval = strtoul(argv[3], 0, 0);
		switch(access_type) {
			case 'b':
				*((unsigned char *) virt_addr) = writeval;
				read_result = *((unsigned char *) virt_addr);
				break;
			case 'h':
				*((unsigned short *) virt_addr) = writeval;
				read_result = *((unsigned short *) virt_addr);
				break;
			case 'w':
				*((unsigned long *) virt_addr) = writeval;
				read_result = *((unsigned long *) virt_addr);
				break;
		}
		printf("Written 0x%X; readback 0x%X\n", writeval, read_result); 
		fflush(stdout);
	}
	
	if(munmap(map_base, MAP_SIZE) == -1) FATAL;
    close(fd);
    return 0;
}

